#include "motor.h"

motor motor_l;
motor motor_r;


void motor_Init()
{
   gpio_init(motorL_turn,GPO,0,GPO_PUSH_PULL);
	 pwm_init(motorL_pwm,5000,0);
	 gpio_init(motorR_turn,GPO,0,GPO_PUSH_PULL);
	 pwm_init(motorR_pwm,5000,0);
	 gpio_init(motorC_turn,GPO,0,GPO_PUSH_PULL);
	 pwm_init(motorC_pwm,5000,0);
}


void motorL_SetSpeed(int pwm)
{
 if(pwm>=0)
 {
    gpio_set_level(motorL_turn,0);
	  pwm_set_duty(motorL_pwm,pwm);
 }else
 {
    gpio_set_level(motorL_turn,1);
	  pwm_set_duty(motorL_pwm,-pwm);
 }

}

void motorC_SetSpeed(int pwm)
{
 if(pwm>=0)
 {
    gpio_set_level(motorC_turn,0);
	  pwm_set_duty(motorC_pwm,pwm);
 }else
 {
    gpio_set_level(motorC_turn,1);
	  pwm_set_duty(motorC_pwm,-pwm);
 }

}

void motorR_SetSpeed(int pwm)
{
 if(pwm>=0)
 {
    gpio_set_level(motorR_turn,0);
	  pwm_set_duty(motorR_pwm,pwm);
 }else
 {
    gpio_set_level(motorR_turn,1);
	  pwm_set_duty(motorR_pwm,-pwm);
 }

}

void Final_Motor_Control(int speed,float k,int limit)
{
	Motor_Control(Limit_int(speed-limit,speed-k*(MID_W-final_mid_line),speed+limit),
	              Limit_int(speed-limit,speed+k*(MID_W-final_mid_line),speed+limit));
}



void Speed_Set(pwm_channel_enum pin1,gpio_pin_enum pin2,int speed,uint8 just,uint8 lose)
{
	if(speed>0)
	{
		pwm_set_duty(pin1,speed);
		gpio_set_level(pin2,just);
	}
	else
	{
		pwm_set_duty(pin1,-speed);
		gpio_set_level(pin2,lose);
	}

}


void Motor_Control(int Speed_L,int Speed_R)
{
	motor_l.target_speed=Speed_L;
	motor_r.target_speed=Speed_R;
	
	
	motor_l.duty=Limit_int(-pid_limit,motor_l.duty+PID_Increase(&motor_pid_l,(float)motor_l.encoder_speed,(float)motor_l.target_speed),pid_limit);
	motor_r.duty=Limit_int(-pid_limit,motor_r.duty+PID_Increase(&motor_pid_r,(float)motor_r.encoder_speed,(float)motor_r.target_speed),pid_limit);
	
	Speed_Set(motorL_pwm,motorL_turn,motor_l.duty,0,1);
	Speed_Set(motorR_pwm,motorR_turn,motor_r.duty,0,1);

}


//------------encoder---------------

void Encoder_Init(void)
{
	//zhong jian encoder
	encoder_dir_init(QTIMER1_ENCODER1,QTIMER1_ENCODER1_CH1_C0,QTIMER1_ENCODER1_CH2_C1);
	//left and right encoder
	encoder_dir_init(QTIMER2_ENCODER1,QTIMER2_ENCODER1_CH1_C3,QTIMER2_ENCODER1_CH2_C4);
}


void Encoder_Data_Get(void)
{
	motor_l.encoder_raw=encoder_get_count(QTIMER1_ENCODER1);
	motor_l.encoder_speed=motor_l.encoder_speed*0.2+motor_l.encoder_raw*0.8;
	motor_l.total_encoder+=motor_l.encoder_raw;
	encoder_clear_count(QTIMER1_ENCODER1);
	
	motor_r.encoder_raw=-encoder_get_count(QTIMER2_ENCODER1);
	motor_r.encoder_speed=motor_r.encoder_speed*0.2+motor_r.encoder_raw*0.8;
	motor_r.total_encoder+=motor_r.encoder_raw;
	encoder_clear_count(QTIMER2_ENCODER1);
	
}



//zhi ling 


//string
void zhixian(void)
{
	while(1)
	{Motor_Control(2000,2000);}
}

